#include <iostream>
#include "kalman_filter_hugo.hpp"

kalman_filter_hugo::kalman_filter_hugo() {
        
        step_time = 0.0333;
        last_update_time = 0;
        priori_state_estimate = {0,0,0};
        posteriori_state_estimate = {0,0,0};
        priori_estimate_covariance <<
        1,0,0,
        0,1,0,
        0,0,1;
        posteriori_estimate_covariance <<
        1,0,0,
        0,1,0,
        0,0,1;
        state_transition_model <<
        1,  step_time,  step_time * step_time / 2.0,
        0,  1,  step_time,
        0,  0,  1;
        process_noise_apply_model << step_time * step_time * step_time / 6.0, 
                                     step_time * step_time / 2.0,
                                     step_time;
        observation_model << 1,  step_time,  step_time * step_time / 2.0;
        observation_noise_covariance << OBSERVATION_NOISE_COVARIANCE;
        process_noise_covariance <<
        1,0,0,
        0,1,0,
        0,0,1;
}

void kalman_filter_hugo::update_kalman_filter(double __step_time__, double measure){    
    if (__step_time__ > 0.18) {
      init_filter_state(measure);
    } else {    
      refresh_step_time(__step_time__);
      refresh_state_transition_model(step_time);
      refresh_measurement(measure);
      refresh_process_noise_apply_model(step_time);
      refresh_observation_model(step_time);
    }

    predict_state_estimate();
    predicted_estimate_covariance();
    update_measurement_residual();
    update_measurement_residual_covariance();
    update_optimal_Kalman_gain();
    update_posteriori_state_estimate();
    update_posteriori_estimate_covariance();

       
}

void kalman_filter_hugo::init_filter_state(double init_measure){
  std::cout << "filter reinit start" << std::endl;
  refresh_step_time(0.0333);
  refresh_measurement(init_measure);
  init_posteriori_state_estimate(measurement(0,0));
  init_posteriori_estimate_covariance(1);
  init_observation_noise_covariance(OBSERVATION_NOISE_COVARIANCE);
  init_process_noise_covariance(PROCESS_NOISE_COVARIANCE);
  refresh_state_transition_model(step_time);
  refresh_process_noise_apply_model(step_time);
  refresh_observation_model(step_time);
  std::cout << "filter reinit over" << std::endl;
}

void kalman_filter_hugo::refresh_step_time(double __step_time__){
  step_time = __step_time__;
  //std::cout << "step_time:\n" << step_time << std::endl;
}

void kalman_filter_hugo::refresh_measurement(double measure){
  measurement << measure;
  std::cout << "measurement:\n" << measurement << std::endl;
}

void kalman_filter_hugo::refresh_state_transition_model(double __step_time__){
  state_transition_model <<
    1, __step_time__, __step_time__ * __step_time__ / 2.0,
    0,         1,       __step_time__,
    0,         0,               1;
  //std::cout << "state_transition_model:\n" << state_transition_model << std::endl;
}

void kalman_filter_hugo::refresh_process_noise_apply_model(double __step_time__){
  process_noise_apply_model <<
  __step_time__ * __step_time__ * __step_time__ / 6,
  __step_time__ * __step_time__ / 2,
  __step_time__;
  //std::cout << "process_noise_apply_model:\n" << process_noise_apply_model << std::endl;
}

void kalman_filter_hugo::refresh_observation_model(double __step_time__){
  //observation_model << 1, __step_time__, __step_time__ * __step_time__ / 2;
  observation_model << 1, 0, 0;
  //std::cout << "observation_model:\n" << observation_model << std::endl;
}

void kalman_filter_hugo::predict_state_estimate(){
  priori_state_estimate = state_transition_model * posteriori_state_estimate;
  std::cout << "priori_state_estimate:\n" << priori_state_estimate << std::endl;
}

void kalman_filter_hugo::predicted_estimate_covariance(){
    priori_estimate_covariance = state_transition_model 
                               * posteriori_estimate_covariance 
                               * state_transition_model.transpose()
                               + 
                                 process_noise_covariance * Eigen::Matrix<double, STATE_COUNT, STATE_COUNT>::Identity();
                               //* process_noise_apply_model 
                               //* process_noise_apply_model.transpose();
  //std::cout << "priori_estimate_covariance:\n" << priori_estimate_covariance << std::endl;
}

void kalman_filter_hugo::update_measurement_residual(){
    measurement_residual = measurement 
                         - 
                           observation_model 
                         * priori_state_estimate;
  std::cout << "measurement_residual:\n" << measurement_residual << std::endl;
}

void kalman_filter_hugo::update_measurement_residual_covariance(){
    measurement_residual_covariance = observation_model 
                                    * priori_estimate_covariance 
                                    * observation_model.transpose()
                                    + 
                                      observation_noise_covariance;
  std::cout << "measurement_residual_covariance:\n" << measurement_residual_covariance << std::endl;
}

void kalman_filter_hugo::update_optimal_Kalman_gain(){
  optimal_Kalman_gain = priori_estimate_covariance
                        * observation_model.transpose()
                        / measurement_residual_covariance.determinant()
                        * measurement_residual_covariance.adjoint();
  //std::cout << "optimal_Kalman_gain:\n" << optimal_Kalman_gain << std::endl;
}

void kalman_filter_hugo::update_posteriori_state_estimate(){ // state update equation
  posteriori_state_estimate = priori_state_estimate
                              + optimal_Kalman_gain
                              * measurement_residual;
  std::cout << "posteriori_state_estimate:\n" << posteriori_state_estimate << std::endl;
}


void kalman_filter_hugo::update_posteriori_estimate_covariance(){
  posteriori_estimate_covariance = (Eigen::Matrix<double, STATE_COUNT, STATE_COUNT>::Identity()
                                   - optimal_Kalman_gain * observation_model)
                                   * priori_estimate_covariance;
  //std::cout << "posteriori_estimate_covariance:\n" << posteriori_estimate_covariance << std::endl;
}

double kalman_filter_hugo::get_estimate_state(){
  return posteriori_state_estimate(0,0);
}

void kalman_filter_hugo::init_posteriori_state_estimate(double init_measure){
  posteriori_state_estimate = {init_measure,0,0};
  std::cout << "init posteriori_state_estimate:\n" << posteriori_state_estimate << std::endl;
}

void kalman_filter_hugo::init_posteriori_estimate_covariance(double scaler){
  posteriori_estimate_covariance <<
        scaler,0,0,
        0,scaler,0,
        0,0,scaler;
  std::cout << "init posteriori_estimate_covariance:\n" << posteriori_estimate_covariance << std::endl;
}

void kalman_filter_hugo::init_observation_noise_covariance(double scaler){
  observation_noise_covariance << scaler;
  std::cout << "init observation_noise_covariance:\n" << observation_noise_covariance << std::endl;
}

void kalman_filter_hugo::init_process_noise_covariance(double scaler){
  process_noise_covariance <<
        scaler,0,0,
        0,scaler,0,
        0,0,scaler;
  std::cout << "init process_noise_covariance:\n" << process_noise_covariance << std::endl;
}

void kalman_filter_hugo::predict_future_state(double future_time){
  refresh_state_transition_model(future_time);
  predict_state = state_transition_model * posteriori_state_estimate;
}

double kalman_filter_hugo::get_predict_state(double future_time){
  predict_future_state(future_time);
  return predict_state(0,0);
}